// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-

/*
 * Copyright (C) 2008 RobotCub Consortium
 * Author: Marco Maggiali, Marco Randazzo, Alessandro Scalzo, Marco Accame
 * CopyPolicy: Released under the terms of the GNU GPL v2.0.
 *
 */

/**
*
@ingroup icub_tools
@ingroup icub_guis
\defgroup icub_canLoader canLoader

Use this application to download your firmware in DSP boards.

\section intro_sec Description
CanLoder20 is a YARP based application that allows the firmware update of the DSP boards of iCub.
The board types currently supported by the CanLoader application are:
- MC4     (Motorola 56807 DSP for control of bushed CC motors)
- BLL     (Motorola 56807 DSP for control of brushless motors)
- STRAIN (DSPIC 30f4013 6 axis force/torque sensor)
- MAIS     (DSPIC 30f4013 Hall-effect position sensor)
_ SKIN   (DSPIC 30f4011 Tactile Sensor board)
In order to successfully transfer the firmware to the boards using the CanLoader application, the desired boards
must be powered on and properly connected to the CAN Bus using either the 'ecan' or 'pcan' or 'cfw2can' or 'socketcan' CAN Bus driver,
or, alternatively, canLoader can program the boards connected to a ETH board.
The CanLoader application can run in two different modalities:
- Graphical User Interface (GUI) modality:
This modality allows the user to examinate the status of the DSP boards detected on the specified CAN Bus, to download
the firmware to multiple bords and to change other settings, such as the individual CAN addresses or the identification labels.
- Command line modality:
This modality allows to the user to update the firmware of one DSP board in a fast and scriptable way.
The application requires in input a set of parameters (specified in the parameters section) in order to identify the board to be updated.
The application returns a value that specifies the result of the download.

\section lib_sec Libraries
YARP libraries.

\section parameters_sec Parameters
You can use the canLoader application in two ways: using a GUI or by command line.
The GUI is executed by default if the canLoader application is launched without parameters.
For example:
./canLoader
executes the graphical version of the canLoader.

To use the canLoader application by command line, without the graphical interface, use the following syntax:
./canLoader --canDeviceType t --canDeviceNum x --boardId y --firmware myFirmware.out.S
./canLoader --canDeviceType ETH --canDeviceNum 1|2 --boardId y --firmware myFirmware.out.S --boardIPAddr aaa.aaa.aaa.aaa
All of the parameters are mandatory. A description of the parameters follows:
--canDeviceType t: specifies the type of canBusDriver used. The parameter t can assume the values 'ecan' or 'pcan' or 'cfw2can' or 'socketcan'
--canDeviceNum x: specifies the canBus identification number. The parameter x can be 0,1,2 or 3.
--boardId y: specifies the can address of the board on which the firmware will be downloaded. The parameter y ranges from 1 to 15.
--firmware myFirmware.out.S: specifies the file name containing the firmware (binary code) that will be downloaded.
--boardIPAddr aaa.aaa.aaa.aaa: it is the ETH board IP address.

\section portsa_sec Ports Accessed
None

\section portsc_sec Ports Created
None

\section in_files_sec Input Data Files
The canLoader application requires in input a file containing the firmware that will be downloaded.
The extension of the firmware file is typically '.out.s' for the Motorola DSP-based boards and '.hex' fot DSPIC-based boards.
The name of the file containing the firmware is specified using the --firmware parameter if the canLoader application is executed by command line
or browsing the directories if the GUI is used.

\section out_data_sec Output Data Files
No output files are generated by the canLoader application.

If the application is executed using the command line modality, the following codes are retuned by the application, identifying the status (success or not) of the download.

#define ALL_OK                        0

#define INVALID_CMD_STRING           -1

#define INVALID_PARAM_CANTYPE        -2

#define INVALID_PARAM_CANNUM         -3

#define INVALID_PARAM_BOARDID        -4

#define INVALID_PARAM_FILE           -5

#define ERR_NO_BOARDS_FOUND         -10

#define ERR_BOARD_ID_NOT_FOUND      -11

#define ERR_UNKNOWN                 -12

#define DOWNLOADERR_NOT_CONNECTED   -20

#define DOWNLOADERR_BOARD_NOT_SEL   -21

#define DOWNLOADERR_FILE_NOT_SEL    -22

#define DOWNLOADERR_FILE_NOT_OPEN   -23

#define DOWNLOADERR_BOARD_NOT_START -24

#define DOWNLOADERR_TRANSFER_ERROR  -25

In this way, an external script that invokes the canLoader application can check the result of the firmware download operation.


\section conf_file_sec Configuration Files
The canLoader application uses a configuration file (config.txt) located in the same diretory of the executable.
The configuration file specifies the filename of the firmware used in the last download operation.
If the configuration file is not found when the canLoader application is executed in graphical modality, a new config.txt is generated.
The configuration file is not used when the canLoader is executed in command line modality.

\section tested_os_sec Tested OS
Linux and Windows.

\section example_sec Example Instantiation of the Module
canLoader
canLoader --help
canLoader --canDeviceType esd --canDeviceNum 2 --boardId 1 --firmware myFirmware.out.S
canLoader --canDeviceType ETH --canDeviceNum 1 --boardId 4 --firmware myFirmware.out.S --boardIPAddr 10.0.1.8

\author Marco Randazzo

Copyright (C) 2008 RobotCub Consortium

CopyPolicy: Released under the terms of the GNU GPL v2.0.

This file can be edited at src/myModule/main.cpp.
**/

#include "downloader.h"
#include "driver.h"
#include <yarp/os/Time.h>
#include <yarp/os/Log.h>
#include <yarp/dev/Drivers.h>

#include <string>   //stl string
#include <string.h> //memcpy ...
#include <stdlib.h>

#include <stdio.h>
#include <fcntl.h>

#include <iCubCanProtocol.h>
#include <iCubCanProto_types.h>

cDownloader downloader;

//can_parameters_type params;
int networkId=0;
int canID=1;
unsigned int  localAddr=0;
unsigned int remoteAddr=0;
const int maxNetworks=10; //max number of can networks
std::string networkType;
bool calibration_enabled=false;
bool prompt_version=false;

//enum
//{
//    COLUMN_SELECTED,
//    COLUMN_ID,
//    COLUMN_TYPE,
//    COLUMN_VERSION,
//    COLUMN_RELEASE,
//    COLUMN_BUILD,
//    COLUMN_SERIAL,
//    COLUMN_STATUS,
//    COLUMN_ADD_INFO,
//    COLUMN_EEPROM,
//    NUM_COLUMNS
//};

#define ALL_OK                         0
#define INVALID_CMD_STRING            -1
#define INVALID_PARAM_CANTYPE        -2
#define INVALID_PARAM_CANNUM        -3
#define INVALID_PARAM_BOARDID        -4
#define INVALID_PARAM_FILE            -5
#define ERR_NO_BOARDS_FOUND            -10
#define ERR_BOARD_ID_NOT_FOUND      -11
#define ERR_UNKNOWN                    -12
#define ERR_NO_NETWORK_INTERFACE     -13
#define DOWNLOADERR_NOT_CONNECTED    -20
#define DOWNLOADERR_BOARD_NOT_SEL    -21
#define DOWNLOADERR_FILE_NOT_SEL    -22
#define DOWNLOADERR_FILE_NOT_OPEN   -23
#define DOWNLOADERR_BOARD_NOT_START -24
#define DOWNLOADERR_TRANSFER_ERROR    -25




//*********************************************************************************


static bool compile_ip_addresses(const char* addr)
{
    ACE_UINT32 ip1,ip2,ip3,ip4;
    sscanf(addr,"%d.%d.%d.%d",&ip1,&ip2,&ip3,&ip4);
    remoteAddr=(ip1<<24)|(ip2<<16)|(ip3<<8)|ip4;

    size_t count=0;
    ACE_INET_Addr* addr_array=NULL;
    int ret=ACE::get_ip_interfaces(count,addr_array);

    if (ret || count<=0)
    {
        return false;
    }

    localAddr=addr_array[0].get_ip_address();

    for (unsigned int a=1; a<count; ++a)
    {
        if ((remoteAddr & 0xFFFF0000)==(addr_array[a].get_ip_address() & 0xFFFF0000))
        {
            localAddr=addr_array[a].get_ip_address();
            break;
        }
    }

    return true;
}


static void start_end_click ()
{
    int ret = 0;

    if (downloader.connected==false)
    {
        yarp::os::Property params;
        params.put("device", networkType.c_str());

        if (networkType=="ETH")
        {
            yError() << "ETH not supported yet" ;
        }
        else
        {
            params.put("canDeviceNum", networkId);
            params.put("canTxQueue", 64);
            params.put("canRxQueue", 64);
            params.put("canTxTimeout", 2000);
            params.put("canRxTimeout", 2000);
        }

        //try to connect to the driver
        ret = downloader.initdriver(params);

        if (ret == -1)
        {
            yError() << "Init driver failed" << "Hardware busy or not connected?!";
            return;
        }

        //drv_sleep (2000);
        //get the infos from the board
        ret = downloader.initschede();

        if (ret == -1)
        {
            yError() << "Communication error" << "No answers received (no boards found).";
            downloader.stopdriver();
            return;
        }

        //enable select/deselect all buttons
        yInfo() << "Driver Connected";
    }
    else
    {
        downloader.stopdriver();
        yInfo() << "Driver Stopped";
    }
}

//*********************************************************************************
static int download_click (std::string* user_data)
{
    double timer_start =0;
    double timer_end   =0;
    // check if can driver is running
    if (downloader.connected == false)
    {
        yError() << "Driver not running " << " Use 'Connect' button to start the driver";
        return DOWNLOADERR_NOT_CONNECTED;
    }

    //check if at least one board was selected
    bool at_least_one_board_selected = false;
    int i        = 0;

    for (i=0; i<downloader.board_list_size; i++)
    {
        if (downloader.board_list[i].status==BOARD_RUNNING &&
            downloader.board_list[i].selected==true)
            at_least_one_board_selected = true;
    }

    if (!at_least_one_board_selected)
    {
        yError() << "No Boards selected! " << "Select one or more boards to download the firmware";
        return DOWNLOADERR_BOARD_NOT_SEL;
    }

    // Open the selected file
    char* buffer=NULL;

    if (user_data==NULL)
    {
        yError() << "No files selected! " << "Select the file You want to download, first";
        return DOWNLOADERR_FILE_NOT_SEL;
    }
    else
    {
        std::string* p = (std::string*)user_data;
        buffer = new char[255];
        strcpy(buffer,(p->c_str()));
    }
    if (downloader.open_file(buffer)!=0)
    {
        yError() << "Error opening the selected file!";
        return DOWNLOADERR_FILE_NOT_OPEN;
    }

    // Get an identification of the firmware fot the file that you have selected
    int firmware_board_type=0;
    int firmware_version=0;
    int firmware_revision=0;

    //indentify download type from the type of the selected boards
    int download_type = icubCanProto_boardType__unknown;
    bool download_eeprom =false;
    for (i=0; i<downloader.board_list_size; i++)
    {
        if (downloader.board_list[i].selected==true)
        {
            download_type = downloader.board_list[i].type;
            download_eeprom = downloader.board_list[i].eeprom;
        }

    }

    // Start the download for the selected boards
    for (i=0; i<downloader.board_list_size; i++)
    {
        if (downloader.board_list[i].status==BOARD_RUNNING &&
            downloader.board_list[i].selected==true)
        {
            if (downloader.startscheda(downloader.board_list[i].bus, downloader.board_list[i].pid, downloader.board_list[i].eeprom, downloader.board_list[i].type)!=0)
            {
                yError() << "Unable to start the board" << "Unable to send message 'start' or no answer received";
                return DOWNLOADERR_BOARD_NOT_START;
            }
            else
            {
                downloader.board_list[i].status=BOARD_WAITING;
            }
        }
    }

    int ret      = 0;
    int finished = 0;

    timer_start= yarp::os::Time::now();

    bool print00 = false, print25 = false, print50 = false, print75 = false, print99 = false;
    // Start the download for the selected boards
    do
    {
        ret = downloader.download_file(CanPacket::everyCANbus, 0x0F, download_type, download_eeprom);
        if (float(downloader.progress)/downloader.file_length >0.0  && print00==false)        {yInfo ("downloading %s, 1%% done\n",buffer); print00=true;}
        if (float(downloader.progress) / downloader.file_length >0.25 && print25 == false)    { yInfo("downloading %s, 25%% done\n", buffer); print25 = true; }
        if (float(downloader.progress) / downloader.file_length >0.50 && print50 == false)    { yInfo("downloading %s, 50%% done\n", buffer); print50 = true; }
        if (float(downloader.progress) / downloader.file_length >0.75 && print75 == false)    { yInfo("downloading %s, 75%% done\n", buffer); print75 = true; }
        if (float(downloader.progress) / downloader.file_length >0.99 && print99 == false)    { yInfo("downloading %s, finished!\n", buffer); print99 = true; }

        if (ret==1)
        {
            //Continuing the download
        }
        if (ret==-1)
        {
            //Fatal Error during download, terminate
            finished = 1;
        }
        if (ret==0)
        {
            //Download terminated
            finished = 1;
        }

    }
    while (finished!=1);
    timer_end= yarp::os::Time::now();

    // End the download for the selected boards
    int errors =0;
    downloader.stopscheda(CanPacket::everyCANbus, 15);

    //Display result message
    if (errors==0)
    {
        char time_text [50];
        double download_time = (timer_end-timer_start) ;
        sprintf (time_text, "All Board OK\nDownload Time (s): %.2f", download_time);
        return ALL_OK;
    }
    else
    {
        return DOWNLOADERR_TRANSFER_ERROR;
    }

}

//*********************************************************************************
bool validate_selection (int wanted_type)
{
    int first_type = icubCanProto_boardType__unknown;
    bool something_selected=false;
    int i=0;
    for (i=0; i<downloader.board_list_size; i++)
    {
        if (downloader.board_list[i].selected==true)
        {
            something_selected=true;
            first_type = downloader.board_list[i].type;
        }
    }
    //check if there is at least one board selected
    if (something_selected == false)
    {
        //if not, this is sthe first board you're going to select,
        //so it's ok, you can select it
        return true;
    }

    //check the compatibility of the board that you selected now, with
    //the previous selected ones. all the boards must be of the same type.
    if (wanted_type != first_type)
    {
        return false;
    }
    else
    {
        return true;
    }
    /*
    switch (wanted_type)
    {
        case BOARD_TYPE_DSP:
        case BOARD_TYPE_2DC:
        case BOARD_TYPE_4DC:
        case BOARD_TYPE_BLL:
            if (first_type == BOARD_TYPE_DSP ||
                first_type == BOARD_TYPE_2DC ||
                first_type == BOARD_TYPE_4DC ||
                first_type == BOARD_TYPE_BLL )
            return true;
            else return false;
        break;
        case BOARD_TYPE_PIC :
        case BOARD_TYPE_SKIN :
        case BOARD_TYPE_STRAIN :
        case BOARD_TYPE_MAIS :
            if (first_type == BOARD_TYPE_PIC  ||
                first_type == BOARD_TYPE_STRAIN ||
                first_type == BOARD_TYPE_MAIS ||
                first_type == BOARD_TYPE_SKIN  )
            return true;
            else return false;
        default:
        case BOARD_UNKNOWN:
             return false;
        break;
    }
    */
    return false;
}


void fatal_error(int err)
{
    switch (err)
    {
        case INVALID_CMD_STRING:
            yError("Unable to parse the command line. The correct format is:\n");
            yError("canLoader --canDeviceType t --canDeviceNum x --boardId y --firmware myFirmware.out.S\n");
            yError("canLoader --canDeviceType ETH --canDeviceNum 1|2 --boardId y --firmware myFirmware.out.S --boardIPAddr aaa.aaa.aaa.aaa\n");
            ::exit(err);
        break;
        case INVALID_PARAM_CANTYPE:
            yError("invalid --canDeviceType parameter \n");
            yError("must be 'ecan' or 'pcan' or 'cfw2' or 'socketcan'\n");
            ::exit(err);
        break;
        case INVALID_PARAM_CANNUM:
            yError("invalid --canDeviceNum parameter \n");
            yError("must be between 0 and 3\n");
            ::exit(err);
        break;
        case INVALID_PARAM_BOARDID:
            yError("invalid --boardId parameter \n");
            yError("must be between 0 and 15\n");
            ::exit(err);
        break;
        case INVALID_PARAM_FILE:
            yError("invalid --firmware parameter \n");
            yError("file not found\n");
            ::exit(err);
        break;
        case ERR_NO_BOARDS_FOUND:
            yError("no boards found \n");
            yError("check canbus cable, power supply connection etc.\n");
            ::exit(err);
        break;
        case ERR_BOARD_ID_NOT_FOUND:
            yError("the specified board is not available \n");
            ::exit(err);
        break;
        case ERR_NO_NETWORK_INTERFACE:
            yError("could not find network interface\n");
            ::exit(err);
        case ERR_UNKNOWN:
        default:
            yError("Unknown error\n");
            ::exit(ERR_UNKNOWN);
        break;

    }

}

//*********************************************************************************
// Entry point for the application
int myMain( int   argc, char *argv[] )
{
    networkType="empty";

    #ifdef USE_ICUB_MOD
    yarp::dev::DriverCollection dev;
    #endif

    if   (argc==2 && strcmp(argv[1],"--calib")==0)
    {
         calibration_enabled=true;
    }
    //yDebug("argc = %d\n",argc);
    else if (argc!=1)
    {
        prompt_version=true;
        yInfo("Initializing prompt version of canLoader...\n");
        if      (argc==2 && strcmp(argv[1],"--help")==0)
        {
            yInfo("CANLOADER APPLICATION V2.9\n");
            yInfo("Syntax:\n");
            yInfo("to execute the command line version of the canLoader:\n");
            yInfo("./canLoader --canDeviceType <t> --canDeviceNum <x> --discover\n");
            yInfo("./canLoader --canDeviceType <t> --canDeviceNum <x> --boardId <y> --firmware myFirmware.out.S\n");
            yInfo("./canLoader --canDeviceType ETH --canDeviceNum 1|2 --boardId <y> --firmware myFirmware.out.S --boardIPAddr <aaa.aaa.aaa.aaa>\n");
            yInfo("parameter <t> is the name of the CAN bus driver. It can be 'ecan' or 'pcan' or 'cfw2can' or 'socketcan'\n");
            yInfo("parameter <x> is the number of the CAN bus (0-9)\n");
            yInfo("parameter <y> is the CAN address of the board (0-14)\n");
            yInfo("parameter <aaa.aaa.aaa.aaa> IP address of the board (ETH boards only)\n");
            ::exit(0);
        }

        //param1 
        if (strcmp(argv[1], "--canDeviceType") != 0)       fatal_error(INVALID_CMD_STRING);

        //param 2
        if (strcmp(argv[2], "ecan") != 0 &&
            strcmp(argv[2], "pcan") != 0 &&
            strcmp(argv[2], "cfw2can") != 0 &&
            strcmp(argv[2], "socketcan") != 0 &&
            strcmp(argv[2], "ETH") != 0)
        {
            fatal_error(INVALID_PARAM_CANTYPE);
        }
        else
        {
            networkType = argv[2];
        }

        //param3
        if (strcmp(argv[3], "--canDeviceNum") != 0)        fatal_error(INVALID_CMD_STRING);

        if       (argc == 6)
        {
                    if (strcmp(argv[5], "--discover") == 0)
                    {
                        start_end_click();
                        ::exit(0);
                    }
        }
        if        (argc==9 || argc==11)
        {
                //yInfo("canLoader --canDeviceType t --canDeviceNum x --boardId y --firmware myFirmware.out.S\n");
                
                if (strcmp(argv[5],"--boardId")!=0 &&
                    strcmp(argv[5],"--discover")!=0)            fatal_error(INVALID_CMD_STRING);

                if (strcmp(argv[7],"--firmware")!=0 &&
                    strcmp(argv[7],"--calibration")!=0 &&
                    strcmp(argv[7],"--firmwareANDeeprom")!=0)    fatal_error(INVALID_CMD_STRING);

                int param_board_id=0;
                std::string param_filename = "empty";
                int temp_val=-1;

                if (networkType=="ETH")
                {
                    if (argc!=11 || strcmp(argv[9],"--boardIPAddr")) fatal_error(INVALID_CMD_STRING);

                    if (!compile_ip_addresses(argv[10]))
                    {
                        fatal_error(ERR_NO_NETWORK_INTERFACE);
                    }
                }

                temp_val=atoi(argv[4]);
                if (temp_val<0 || temp_val>9)
                    {
                        fatal_error(INVALID_PARAM_CANNUM);
                    }
                else
                    {
                        canID=networkId=temp_val;
                    }

                temp_val=atoi(argv[6]);
                if (temp_val<0 || temp_val>15)
                    {
                        fatal_error(INVALID_PARAM_BOARDID);
                    }
                else
                    {
                        param_board_id=temp_val;
                    }

                param_filename=argv[8];
                std::fstream filestr;
                filestr.open ( param_filename.c_str(), std::fstream::in);
                if (!filestr.is_open())
                    {
                        //file not exists
                        fatal_error(INVALID_PARAM_FILE);
                    }
                else
                    {
                        filestr.close();
                    }
                yInfo("Selecting canDeviceType %s canDeviceNum %d, boardId %d, firmware %s\n", networkType.c_str(), networkId, param_board_id, param_filename.c_str());

                //eeprom check
                bool use_eeprom=false;
                if (strcmp(argv[7],"--firmwareANDeeprom")==0)
                {
                    yInfo("EEprom flag is active");
                    use_eeprom=true;
                }

                start_end_click ();
                if (downloader.board_list_size==0)
                    {
                        fatal_error(ERR_NO_BOARDS_FOUND);
                    }

                int i;
                bool one_selected=false;
                for (i = 0; i < downloader.board_list_size; i++)
                    {
                        if (downloader.board_list[i].pid==param_board_id)
                            {
                                downloader.board_list[i].selected=true;
                                one_selected=true;
                                if (use_eeprom==true) downloader.board_list[i].eeprom=true;
                            }
                    }
                if (one_selected==false)
                    {
                        fatal_error(ERR_BOARD_ID_NOT_FOUND);
                    }

                int ret=download_click(&param_filename);
                if (ret==ALL_OK)
                    yInfo("Program terminated successfully!");
                else
                    yError("Program terminated, DOWNLOAD FAILED!");
                ::exit(ret);
        }
        if        (argc!=9)
        {
          //      fatal_error(INVALID_CMD_STRING);
        }
    }
    else
    {
        yInfo("CANLOADER APPLICATION V2.9\n");
        yInfo("Syntax:\n");
        yInfo("to execute the command line version of the canLoader:\n");
        yInfo("./canLoader --canDeviceType <t> --canDeviceNum <x> --discover\n");
        yInfo("./canLoader --canDeviceType <t> --canDeviceNum <x> --boardId <y> --firmware myFirmware.out.S\n");
        yInfo("./canLoader --canDeviceType ETH --canDeviceNum 1|2 --boardId <y> --firmware myFirmware.out.S --boardIPAddr <aaa.aaa.aaa.aaa>\n");
        yInfo("parameter <t> is the name of the CAN bus driver. It can be 'ecan' or 'pcan' or 'cfw2can' or 'socketcan'\n");
        yInfo("parameter <x> is the number of the CAN bus (0-9)\n");
        yInfo("parameter <y> is the CAN address of the board (0-14)\n");
        yInfo("parameter <aaa.aaa.aaa.aaa> IP address of the board (ETH ethernet boards only)\n");
        ::exit(0);
    }

    return 0;
}

int main(int argc, char* argv[])
{
    return myMain(argc, argv);
}

